#ifndef CONFIG_HPP
#define CONFIG_HPP

#include <string>
#include <vector>
#include <iostream>
#include <cmath>
#define Stop_status 0 //1=车轮不动 0=正常行驶
using namespace std;
// 定义结构体（与C++原定义一致）
struct serial_config {
    int Id;
    int delay;
    int direction; // 1=正转 0=反转
};
struct Motor_Info {
    int16_t speed_set;
    int16_t speed_cur;
    int16_t rec_times;
};
struct YG_value
{
  int Left_horiz;
  int Left_vertic;
  int Right_horiz;
  int Right_vertic;
  int key1;
  int key2;
  int channel;
};
struct Motor_module {
    int16_t module_speed_set;
    int16_t speed_set_L;
    int16_t speed_set_R;
    int angle_set;
    int angle_cur;
       // 自定义默认构造函数，初始化所有成员为0
    Motor_module() : 
        module_speed_set(0), 
        speed_set_L(0), 
        speed_set_R(0), 
        angle_set(0), 
        angle_cur(0) {}
};
class Motor_Config {
public:
    Motor_Config(){};
    ~Motor_Config(){};
    double wheel_radius = 0.1;  //轮子半径，单位米
    double wheel_base = 0.6;    //轮子间距，单位米
    double wheel_track = 1.2;    //轮子轴距，单位米
    vector<serial_config> motor_drv{
    {0, 100,1}, {0, 100,1}, {0, 100,1}, {0, 100,1},
                {0, 100,1}, {0, 100,1}, {0, 100,1}, {0, 100,1}
            };
 
    vector<serial_config> sensor_drv{  //{0,100,0} 最后一个数这里没用
    {0, 100,0}, {0, 100,0}, {0, 100,0}, {0, 100,0},
    {0, 100,0}, {0, 100,0}, {0, 100,0}, {0, 100,0}
            };
    vector<serial_config> bianma_drv{  //{0,100,0} 最后一个数这里没用
    {0, 100,0}, {0, 100,0}, {0, 100,0},{0, 100,0}
            };        
    vector<int>bianma_zero{0,0,0,0};        
    vector<Motor_Info> motor_info{  //电机 设定速度  实际速度 接收次数
        {0,0,0},{0,0,0},{0,0,0},{0,0,0},
        {0,0,0},{0,0,0},{0,0,0},{0,0,0}
    }; 
    vector<Motor_module> motor_module{  //电机模块
        Motor_module(), Motor_module(), Motor_module(), Motor_module()
    };
    vector<int> bianma_cur {0,0,0,0};       
    vector<int> bianma_rectimes {0,0,0,0}; 
    int yaokong_channel=0;
    int yaokong_times=0;
    string serial_port0_;
    int baud_rate0_;
    string serial_port1_;
    int baud_rate1_;
    YG_value yaokong_value{0,0,0,0,0,0,0};
   // vector<int16_t> motor_speed{0,0,0,0,0,0,0,0};
    void yaokonglun(void) //测试用
    {
      for(int i=0;i<8;i++)
      {
          motor_info[i].speed_set = yaokong_value.Left_vertic*5;
      }
     // motor_speed[3]=300;

    }
    void yaokongline(void) //测试用
    {
        double vx,vy,vz;
      //  int yaox,yaoy
      vy = yaokong_value.Left_horiz*0.8/128.0;
      vx = yaokong_value.Left_vertic*0.8/128.0;
      vz = -yaokong_value.Right_horiz*0.5/128.0;
        calculate_module_speed(vx, vy, vz);
    }
      
    
    void calculate_module_speed(double Lx, double Ly, double Oz)
    {
        // 计算每个轮子的速度
        // 假设机器人有四个轮子，编号为0、1、2、3，分别位于机器人的前左、前右、后左、后右位置
        // Vx: 前进速度，Vy: 横向速度，Wz: 旋转速度
         double angle_c[4],line_c[4];
         double Vx,Vy,Wz;
         if(abs(Oz)>=0.001) { Vx=Lx; Vy=0;Wz=Oz;}
           else {Vx=Lx; Vy=Ly; Wz=0;}
        std::cout<<"wz="<<Wz<<std::endl;
        if(abs(Wz)<0.001)
        {
            Wz=0;
     
                 double ag=-atan2(Vy, Vx)*180.0/M_PI; // 计算运动方向角度
                double v=sqrt(Vx*Vx+Vy*Vy); // 计算运动速度大小
                if(ag>95.0)
                {
                    v=-v;
                    ag=ag-180.0;
                }
                else if(ag<-95.0)
                {
                    v=-v;
                    ag=ag+180.0;
                }
               // double angle_convert = ag-180.0 ; // 角度转换系数
                double wheel_speed = (v / wheel_radius)*(30.0/M_PI); // 计算轮子速度
           
            for(int i=0;i<4;i++)
            {
              motor_module[i].module_speed_set= wheel_speed*10;
              //(Vx - Vy) / wheel_radius;  
             // motor_module[i].module_speed_set=(line_c[i] / wheel_radius)*(30.0/M_PI); 
              motor_module[i].angle_set=ag*32768/180.0;
            }
           // std::cout << "Vx: " << Vx << ", Vy: " << Vy << ", Wz: " << Wz << std::endl;
            std::cout << "运动方向角度: " << ag << ", 运动速度: v=" <<v<<", wheel_speed=" << wheel_speed *10<< std::endl;
        } 
        else
        {
            //double angle_c[4],line_c[4];
            // double RX = wheel_base / 2.f;
            // double RY = wheel_track / 2.f;
            // double wmga_max = 0.5;
            // double wmga = Wz;
            // //std::cout<< "RY= "<<RY<<" RX= "<<RX<<" wmga= "<<wmga<<std::endl;
            // if (wmga > wmga_max) wmga = wmga_max;
            // if (wmga < -wmga_max) wmga = -wmga_max;
            // if(Vx<0)wmga=-wmga; //如果Vx<0 逆时针转
            //  //驱动算法矩阵B=AX，计算结果为矢量                 
            // double vfr_x = Vx + RX * wmga;
            // double vfr_y = Vy + RY * wmga;
            // double vfl_x = Vx - RX * wmga;
            // double vfl_y = Vy + RY * wmga;
            // double vrl_x = Vx - RX * wmga;
            // double vrl_y = Vy - RY * wmga;
            // double vrr_x = Vx + RX * wmga;
            // double vrr_y = Vy - RY * wmga;
            // angle_c[1] = atan2(vfl_y, vfl_x) * 180 / M_PI; 
            // angle_c[0] = atan2(vfr_y, vfr_x) * 180 / M_PI;
            // angle_c[3] = atan2(vrl_y, vrl_x) * 180 / M_PI;
            // angle_c[2] = atan2(vrr_y, vrr_x) * 180 / M_PI;
            // line_c[1] = calcDistance(vfl_x, vfl_y);
            // line_c[0] = calcDistance(vfr_x, vfr_y);
            // line_c[3] = calcDistance(vrl_x, vrl_y);
            // line_c[2] = calcDistance(vrr_x, vrr_y);
            if(abs(Vx)<0.001) //原地转
            {
                double RY = wheel_base / 2.f;
                double RX = wheel_track / 2.f;
                double wmga_max = 0.5;
                double wmga = Wz;
                //std::cout<< "RY= "<<RY<<" RX= "<<RX<<" wmga= "<<wmga<<std::endl;
                if (wmga > wmga_max) wmga = wmga_max;
                if (wmga < -wmga_max) wmga = -wmga_max;
                if(Vx<0)wmga=-wmga; //如果Vx<0 逆时针转
                //驱动算法矩阵B=AX，计算结果为矢量                 
                double vfr_x = Vx + RX * wmga;
                double vfr_y = Vy + RY * wmga;
                double vfl_x = Vx - RX * wmga;
                double vfl_y = Vy + RY * wmga;
                double vrl_x = Vx - RX * wmga;
                double vrl_y = Vy - RY * wmga;
                double vrr_x = Vx + RX * wmga;
                double vrr_y = Vy - RY * wmga;
                angle_c[1] = atan2(vfl_y, vfl_x) * 180 / M_PI; 
                angle_c[0] = atan2(vfr_y, vfr_x) * 180 / M_PI;
                angle_c[3] = atan2(vrl_y, vrl_x) * 180 / M_PI;
                angle_c[2] = atan2(vrr_y, vrr_x) * 180 / M_PI;
                line_c[1] = calcDistance(vfl_x, vfl_y);
                line_c[0] = calcDistance(vfr_x, vfr_y);
                line_c[3] = calcDistance(vrl_x, vrl_y);
                line_c[2] = calcDistance(vrr_x, vrr_y);
                for(int i=0;i<4;i++)
                {
                    if(angle_c[i]>90.0)angle_c[i]=angle_c[i]-180.0;
                        else if(angle_c[i]<-90.0)angle_c[i]=angle_c[i]+180.0;

                    if(angle_c[i]>=0)angle_c[i]=90.0-angle_c[i];
                        else angle_c[i]=-90.0-angle_c[i];   
                    if(Wz<0)line_c[i]=-line_c[i]; //如果Wz<0 逆时针转    
                  // std::cout << "轮子" << i << " 角度: " << angle_c[i] << ", 距离: " << line_c[i] << std::endl;
                }
                line_c[1]= -line_c[1];
                line_c[3]= -line_c[3];
                 for(int i=0;i<4;i++)
                {
                motor_module[i].module_speed_set=(line_c[i] / wheel_radius)*(30.0/M_PI)*10;  
                motor_module[i].angle_set=angle_c[i]*32768/180.0;
                std::cout << "轮子" << i << " 角度: " << angle_c[i] << ", rmp: " << motor_module[i].module_speed_set << std::endl;      
                }
            }
            else  //麦克轮
            {
                double RX = wheel_base / 2.f;
                double RY = wheel_track / 2.f;
                double wmga_max = 0.5;
                double wmga = Wz;
                //std::cout<< "RY= "<<RY<<" RX= "<<RX<<" wmga= "<<wmga<<std::endl;
                if (wmga > wmga_max) wmga = wmga_max;
                if (wmga < -wmga_max) wmga = -wmga_max;
                if(Vx<0)wmga=-wmga; //如果Vx<0 逆时针转
                //驱动算法矩阵B=AX，计算结果为矢量                 
                double vfr_x = Vx + RX * wmga;
                double vfr_y = Vy + RY * wmga;
                double vfl_x = Vx - RX * wmga;
                double vfl_y = Vy + RY * wmga;
                double vrl_x = Vx - RX * wmga;
                double vrl_y = Vy - RY * wmga;
                double vrr_x = Vx + RX * wmga;
                double vrr_y = Vy - RY * wmga;
                angle_c[1] = atan2(vfl_y, vfl_x) * 180 / M_PI; 
                angle_c[0] = atan2(vfr_y, vfr_x) * 180 / M_PI;
                angle_c[3] = atan2(vrl_y, vrl_x) * 180 / M_PI;
                angle_c[2] = atan2(vrr_y, vrr_x) * 180 / M_PI;
                line_c[1] = calcDistance(vfl_x, vfl_y);
                line_c[0] = calcDistance(vfr_x, vfr_y);
                line_c[3] = calcDistance(vrl_x, vrl_y);
                line_c[2] = calcDistance(vrr_x, vrr_y);
                for(int i =0; i<4; i++)
                {
                if(Vx<0)
                {
                    line_c[i]=-line_c[i];
                } 
                if(angle_c[i]>92.0)
                {
                    angle_c[i]=angle_c[i]-180.0;
                }
                else if(angle_c[i]<-92.0)
                {
                    angle_c[i]=angle_c[i]+180.0;
                }
                //std::cout << "轮子" << i << " 角度: " << angle_c[i] << ", 距离: " << line_c[i] << std::endl;
                }

                for(int i=0;i<4;i++)
                {
                motor_module[i].module_speed_set=(line_c[i] / wheel_radius)*(30.0/M_PI)*10;  
                motor_module[i].angle_set=angle_c[i]*32768/180.0;
               // motor_module[i].angle_set=0;
                std::cout << "麦克轮轮子" << i << " 角度: " << angle_c[i] << ", rmp: " << motor_module[i].module_speed_set << std::endl;    
                }
            }

        }
        if(Stop_status==1) //车轮不动
        {
            for(int i=0;i<4;i++)
            {
              motor_module[i].module_speed_set=0;
            }
        }
    }

    double calcDistance(double x, double y)
    {
        return sqrt(x * x + y * y);
    }
};
#endif // CONFIG_HPP